/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Joystick;

/**
 * Class to manage controls systems for robot. NOTE: all methods output left
 * and right motor values, and should be fed to TankDrive (if using robotdrive)
 * @author robert
 */
public class PhoenixControls {
    public Joystick leftStick;
    public Joystick rightStick;
    public Joystick gamePad;
    private DriverStationLCD dsLcd = DriverStationLCD.getInstance();
    Const constants;

    /**
     * takes IDs for left and right sticks as well as gamepad
     * @param leftStickChannel
     * @param rightStickChannel
     * @param gamePadChannel
     */
    public PhoenixControls (Joystick leftStick, Joystick rightStick,
            Joystick gamePad, Const constants) {
        this.constants = constants;
        this.leftStick = leftStick;
        this.rightStick = rightStick;
        this.gamePad = gamePad;
    }

    public void putLine (DriverStationLCD.Line line, String message) {
        dsLcd.println(line, 1, message);
    }

    public double throttleLinearized() {
        return ((rightStick.getAxis(Joystick.AxisType.kZ)*-1)+1)/2;
    }

    /**
     * returns joystickValues for tank drive
     * @return values for tankdrive
     */
    public JoystickValues tankDrive() {
        JoystickValues values = new JoystickValues();
        // scale joystick outputs by throttle (Z axis)
        values.leftValue = leftStick.getAxis(Joystick.AxisType.kY) *
                throttleLinearized();
        values.rightValue = rightStick.getAxis(Joystick.AxisType.kY) *
                throttleLinearized();
        values.reverseTriggered = leftStick.getRawButton(2)
                && rightStick.getRawButton(2);

        return values;
    }

    /**
     * returns joystickValues for arcade drive
     * @return values for tankdrive
     */
    public JoystickValues arcadeDrive() {
        JoystickValues values = new JoystickValues();
        double directionalValue = leftStick.getAxis(Joystick.AxisType.kY);
        double twistModifier = leftStick.getAxis(Joystick.AxisType.kX);
        double leftValue = directionalValue + twistModifier * -1;
        if(leftValue > 1) {leftValue = 1;}
        else if(leftValue < -1) {leftValue = -1;}
        double rightValue = directionalValue + twistModifier;
        if(rightValue > 1) {rightValue = 1;}
        else if(rightValue < -1) {rightValue = -1;}
        values.leftValue = leftValue * throttleLinearized();
        values.rightValue = rightValue * throttleLinearized();
        values.reverseTriggered = leftStick.getRawButton(2)
                && rightStick.getRawButton(2);
        
        return values;
    }

    /**
     * returns joystickValues for gamepad drive
     * @return values for tankdrive
     */
    public JoystickValues gamepadDrive() {
        JoystickValues values = new JoystickValues();
        values.leftValue = gamePad.getAxis(Joystick.AxisType.kY);
        values.rightValue = gamePad.getAxis(Joystick.AxisType.kTwist);

        return values;
    }

    /**
     * gamepad controls for lifter
     * @return controls values for lifter
     */
    public LifterValues gamepadLifter() {
        LifterValues values = new LifterValues();
        values.lifterValue = gamePad.getAxis(Joystick.AxisType.kY) * -1;
        values.armValue = gamePad.getAxis(Joystick.AxisType.kThrottle) * -1;
        values.clawOpen = gamePad.getRawButton(constants.ENABLECLAWGAMEPADBUTTON()) |
                rightStick.getRawButton(constants.ENABLECLAWJOYSTICKBUTTON());

        return values;
    }

    public LifterValues joystickLifter() {
        LifterValues values = new LifterValues();
        values.lifterValue = leftStick.getAxis(Joystick.AxisType.kY);
        values.armValue = rightStick.getAxis(Joystick.AxisType.kY);

        return values;
    }

    public MinibotValues gamepadMinibot() {
        MinibotValues values = new MinibotValues();
        if (gamePad.getRawButton(constants.MINIBOTBUTTON1()) &&
                gamePad.getRawButton(constants.MINIBOTBUTTON2())) {
            values.minibotDeployed = true;
        }

        return values;
    }

    /**
     * class for storing motor output
     */
    public class JoystickValues {
        public double rightValue = 0;
        public double leftValue = 0;
        public boolean reverseTriggered = false;
    }

    /**
     * class for storing claw output
     */
    public class LifterValues {
        public double lifterValue;
        public double armValue;
        public boolean clawOpen = false;
    }

    /**
     * class for storing minibot output
     */
    public class MinibotValues {
        public boolean minibotDeployed = false;
    }
}
